#include "AttachedObjectUpdater.h"
#include "Convert.h"
#include "GameMotionState.h"


AttachedObjectUpdater::AttachedObjectUpdater(void)
{
}


AttachedObjectUpdater::~AttachedObjectUpdater(void)
{
}

void AttachedObjectUpdater::attachObjectToBone(BaseObjectPtr obj, Ogre::Node *node, Ogre::Bone* bone, btTransform &trans)
{
	mBones[obj] = BoneTuple(node, bone, btTransform(trans));
}

void AttachedObjectUpdater::attachObjectToObject(BaseObjectPtr obj, BaseObjectPtr attachedTo, btTransform &trans)
{
	mObjects[obj] = ObjectTuple(attachedTo, btTransform(trans));
}

void AttachedObjectUpdater::removeObject(BaseObjectPtr obj)
{
	mBones.erase(obj);
	mObjects.erase(obj);
}
void AttachedObjectUpdater::updateObjects(void)
{
	for(BoneMap::iterator it = mBones.begin();it != mBones.end();++it)
	{
		it->second.get<0>()->_update(true,true);
		Ogre::Vector3 vec = getWorldPosition(it->second.get<0>(), it->second.get<1>());
		Ogre::Quaternion quat = getWorldOrientation(it->second.get<0>(), it->second.get<1>());

		btTransform trans = cvt(quat, vec) * it->second.get<2>();
		it->first->getBody()->setWorldTransform(trans);
		it->first->getBody()->getMotionState()->setWorldTransform(trans);

		it->first->getBody()->activate();
		/*it->first->getBody()->clearForces();
		it->first->getBody()->setLinearVelocity(btVector3(0.f, 0.f, 0.f));
		it->first->getBody()->setAngularVelocity(btVector3(0.f, 0.f, 0.f));
		it->first->getBody()->setGravity(btVector3(0.f, 0.f, 0.f));*/
	}
	for(ObjectMap::iterator it = mObjects.begin();it != mObjects.end();++it)
	{
		btTransform trans = it->second.get<0>()->getBody()->getWorldTransform() * it->second.get<1>();
		it->first->getBody()->setWorldTransform(trans);
		it->first->getBody()->getMotionState()->setWorldTransform(trans);

		it->first->getBody()->activate();
		/*it->first->getBody()->clearForces();
		it->first->getBody()->setLinearVelocity(btVector3(0.f, 0.f, 0.f));
		it->first->getBody()->setAngularVelocity(btVector3(0.f, 0.f, 0.f));
		it->first->getBody()->setGravity(btVector3(0.f, 0.f, 0.f));*/
	}
}
Ogre::Quaternion AttachedObjectUpdater::getWorldOrientation(Ogre::Node *node, Ogre::Bone* bone)
{
	return node->_getDerivedOrientation() *(bone->_getDerivedOrientation());
}

Ogre::Vector3 AttachedObjectUpdater::getWorldPosition(Ogre::Node *node, Ogre::Bone* bone)
{
	return node->_getFullTransform() * bone->_getDerivedPosition();
}

Ogre::Matrix4 AttachedObjectUpdater::getWorldTransformation(Ogre::Node *node, Ogre::Bone* bone)
{
	return node->_getFullTransform() * bone->_getFullTransform();
}

void AttachedObjectUpdater::clearList(void)
{
	mBones.clear();
	mObjects.clear();
}